#include "camera.h"
/*

* white_point_value是在赛道上遍历采集多幅图像（遍历mt9v03x_image_dvp数组），取每幅图像的中位数，再比较这些中位数大概作为阈值
* Color_threshold和上面同样的方法（图像上所有像素点经过差比和运算后的值）
* 当然，采集的值用脚本画图或者excel画图会明显看出阈值

* mt9v03x_image_dvp[120][188]
* point是一个结构体，包含行和列的坐标值

* 参考文档：https://mp.weixin.qq.com/s/5s1UPwL9bqZVcKJ_pUkk1A
*/

/********************参考阈值********************/
#define  white_point_value  230 //白色参考值
#define  Color_threshold    9  // 差比和阈值
/*********************over*********************/

/**
 * @brief 算出每个点的差比和
 *
 * @param a 一个点的像素值
 *
 * @param b 另一个点的像素值
 *
 * @return 返回一个点差比和运算后的结果
 */
static uint8 Comparison_and_comparison(uint8 a,uint8 b)
{
    int16 sum;//和
    int16 diff;//差
    uint8 position;//像素点位置
    sum = a + b;//计算像素点之和
    diff = myabs(a - b);//计算像素点之差
    if(sum>0)//边界保护//等于0此时无边界保护
    {
        position = (uint8)((diff<<7)/(sum+1)); //计算差比和数值 并放大128倍  +1避免除数为0
        return position;
    }
    else return 0;
}

/**
 * @brief 从下往上找到图像上最长的一列 row行，col列
 *
 * @param none
 *
 * @return 返回找到的最长白列顶点的坐标
 */
static point bottom_find_black(void)
{
    uint8 position,position1;//像素点位置的差比和
    point min_point;
    uint8 row=119,col=0;
    min_point.row=119;
    uint8 l_col=4,r_col=184;
    for (uint8 j = l_col; j < r_col; j+=5)//遍历宽188*高120的灰度图像
    {
        mt9v03x_image_dvp[1][j]=0;
        for (uint8 i = 115; i > 0; i--)
        {
            position = Comparison_and_comparison(mt9v03x_image_dvp[i+4][j], mt9v03x_image_dvp[i][j]);
            position1 = Comparison_and_comparison(mt9v03x_image_dvp[119][j], white_point_value);
            if (position > Color_threshold && position1 < Color_threshold) //边界阈值
            {
                if(i < row)//小于最小值
                {
                    min_point.row=row;
                    min_point.col=col;
                    row=i;
                    col=j;
                }
                else if(i < min_point.row && i >= row)
                {
                    min_point.row=i;
                    min_point.col=j;
                }
                break;
            }
        }
    }
    return min_point;
}

/**
 * @brief 从右往左寻找左边界 row行，col列
 *
 * @param p 存放左边界点坐标内存的指针
 *
 * @param row 寻找左边界结束的列坐标
 *
 * @param col 寻找左边界开始的行坐标
 * 
 * @param n_left 寻找到的左边界点个数
 * 
 * @param good_nl 寻找到的左边界点个数（不包括图像最两边的点，图像会出现某行全白的情况）
 *
 * @return none
 */
static void find_edge_left(point *p,uint8 row,uint8 col,uint8 *n_left,uint8 *good_nl)
{
    uint8 position;//像素点位置的差比和
    uint8 n=0,n1=0;
    for (uint8 i = 119; i > row; i--)
    {
        for (uint8 j = col; j >= 4; j--)
        {
            position = Comparison_and_comparison(mt9v03x_image_dvp[i][j-4], mt9v03x_image_dvp[i][j]);
            if (position > Color_threshold) //黑点阈值
            {
                (p + n)->row = i;
                (p + n)->col = j-4;//列坐标
                n++;
                n1++;
                break;
            }
            if(position <= Color_threshold && j == 4)//补线
            {
                (p + n)->row = i;
                (p + n)->col = j-4;
                n++;
                break;
            }
        }
    }
    *n_left = n;
    *good_nl = n1;
}

/**
 * @brief 从左往右寻找右边界 row行，col列
 *
 * @param p 存放右边界点坐标内存的指针
 *
 * @param row 寻找右边界开始的列坐标
 *
 * @param col 寻找右边界结束的行坐标
 * 
 * @param n_left 寻找到的右边界点个数
 * 
 * @param good_nl 寻找到的右边界点个数（不包括图像最两边的点，图像会出现某行全白的情况）
 *
 * @return none
 */
static void find_edge_right(point *p,uint8 row,uint8 col,uint8 *n_right,uint8 *good_nr)//寻找右边界 //row行，col列
{
    uint8 position;//像素点位置的差比和
    uint8 n=0,n1=0;
    for (uint8 i = 119; i >= row; i--)
    {
        for (uint8 j = col; j < 184; j++)
        {
            position = Comparison_and_comparison(mt9v03x_image_dvp[i][j+4], mt9v03x_image_dvp[i][j]);
            if (position > Color_threshold) //黑点阈值j==187||
            {
                (p + n)->row = i;
                (p + n)->col = j+4;//横坐标
                n++;
                n1++;
                break;
            }
            if(position <= Color_threshold && j == 183)//补线
            {
                (p + n)->row = i;
                (p + n)->col = j+4;//横坐标
                n++;
                break;
            }
        }
    }
//    printf("right:%d\n\r",n1);
    *n_right = n;
    *good_nr = n1;
}

